# Setup code
%matplotlib inline
import matplotlib.pyplot as plt
import numpy as np
from numerics.matte4.ode import ode_adaptive, ode_solver, implicit_euler
from collections import deque
import numerics.numerical_integration as num_integ
import random
## For animations
from matplotlib import animation, rc
from IPython.display import HTML
##
from numerics.tracker.interpolate_tracker_data import iptrack as trackerData, iptrack_p1100902 as tracker_1, load_p1100902
import numerics.tracker.filter_values as filter_values
TRACK_START_X = 0
TRACK_END_X = 2
run_slow = False
Displays a track polynomial from tracker data
def trackLoading():
(ball_x, ball_y), polynomial, track_end_x = tracker_1() #trackerData("test_data/mass_a.txt")
steps = 100
x = np.linspace(0, track_end_x, steps)
y = np.polyval(polynomial, x)
plt.figure(figsize=(12,6))
plt.plot(x, y, linewidth=2, label="Track")
plt.scatter(ball_x, ball_y, label="Ball start")
plt.legend()
plt.show()
trackLoading()
Let's try animation with a very simple parabola, $x^2 - 1.5x + 0.57$
# Just showing the curve
def simpleCurve():
parabola = [1, -1.5, 0.57]
d_parabola = np.polyder(parabola)
steps = 100
framerate = 20/1000
frame_count = 300
x_pos = [0]
vel = [0]
for i in range(frame_count-1):
time = i*framerate
# Bruker matte4-funksjoner, heh
# part 1
# v'=sin(alpha)*9.81
# where functions ode_adaptive ask for x, think t for time instead.
current_x = x_pos[-1]
def accell(t, v):
track_angle = np.arctan(-np.polyval(d_parabola, current_x))
return np.sin(track_angle)*9.81 - 0.8*vel[-1]
t_num, v_num = ode_adaptive(accell, x0=0, xend=framerate, y0=vel[-1], h0=0.2)
vel.append(v_num[-1])
#part 2
def velocity(t, x):
return v_num[-1]
t_num, x_num = ode_adaptive(velocity, x0=0, xend=framerate, y0=current_x, h0=0.2)
x_pos.append(x_num[-1])
# END matte 4
x = np.linspace(0, 1.5, steps)
y = np.polyval(parabola, x)
time_axis = np.linspace(0, frame_count*framerate, frame_count)
# Plot
fig, (x_ax, t_ax) = plt.subplots(2, figsize=[12, 8])
x_ax.plot(x, y, label=f"{parabola[0]}x^2 + {parabola[1]}x + {parabola[2]}")
x_ax.plot(x, np.arctan(-np.polyval(d_parabola, x)), label="Track Angle (rad)")
x_ax.plot(x, -np.polyval(d_parabola, x), label="Track derivative (negated)")
x_ax.plot(x, np.sin(np.arctan(-np.polyval(d_parabola, x))), label="Track Angle cosine")
x_ax.legend()
t_ax.plot(time_axis, vel, label="Ball velocity")
t_ax.plot(time_axis, x_pos, label="Ball x")
t_ax.legend()
#plt.show()
simpleCurve()
def curveAnimation():
fig, ax = plt.subplots()
plt.xlim((0, 1.5))
# Background path using the curve
parabola = [1, -1.5, 0.57]
d_parabola = np.polyder(parabola)
steps = 100
x = np.linspace(0, 1.5, steps)
y = np.polyval(parabola, x)
plt.plot(x, y, label=f"{parabola[0]}x^2 + {parabola[1]}x + {parabola[2]}")
plt.legend()
# Animated line
ax.set_xlim(( 0, 1.5))
ax.set_ylim((0, 1))
line, = ax.plot([], [], linestyle='None', marker="o", label="Frame: 0")
ax.legend()
# Animation settings
framerate = 16/1000
frame_count = 350
print(f"Total duration of animation is {framerate*frame_count}s")
def init():
line.set_data([], [])
return (line,)
x_pos = [0]
y_pos = [np.polyval(parabola, x_pos[0])]
vel = [0]
def animate(frame):
# Bruker matte4-funksjoner, heh
# part 1 - integrate acceleration to get velocity
# v'=sin(alpha)*9.81
# where functions ode_adaptive ask for x, think t for time instead.
current_x = x_pos[-1]
def accell(t, v):
track_angle = np.arctan(-np.polyval(d_parabola, current_x))
return np.sin(track_angle)*9.81 - 0.8*vel[-1]
t_num, v_num = ode_adaptive(accell, x0=0, xend=framerate, y0=vel[-1], h0=framerate/2)
# Save velocity
vel.append(v_num[-1])
#part 2 - integrate velocity to get position
def velocity(t, x):
return v_num[-1]
t_num, x_num = ode_adaptive(velocity, x0=0, xend=framerate, y0=current_x, h0=framerate)
# Save the values
x_pos.append(x_num[-1])
y_pos.append(np.polyval(parabola, x_num[-1]))
# Update the animated point
line.set_data(x_pos[-1], y_pos[-1])
line.set_label(f"Frame {frame}, t={np.round(frame*framerate, 1)}. pos=({round(x_pos[-1],1)}, {round(y_pos[-1],1)}) ")
ax.legend()
return (line,) # Return all updated artists
anim = animation.FuncAnimation(fig, animate, init_func=init,
frames=frame_count, interval=framerate*1000,
blit=True)
rc('animation', html='jshtml')
return anim
anim = curveAnimation()
save_vid = False
if save_vid:
# Set up formatting for the movie files
Writer = animation.writers['ffmpeg']
writer = Writer(fps=1/framerate, metadata=dict(artist='Kristian'), bitrate=1800)
anim.save('out/1-simple-curve-ball.mp4', writer=writer)
print("Saved!")
if run_slow:
anim
# Curve with obstacle animation
def obstacleCurveAnimation(parabola):
fig, ax = plt.subplots()
plt.xlim((TRACK_START_X, TRACK_END_X))
# Background path using the curve
d_parabola = np.polyder(parabola)
steps = 100
x = np.linspace(TRACK_START_X, TRACK_END_X, steps)
y = np.polyval(parabola, x)
label = " ".join([f"{round(c, 1)}x^{len(parabola)-i}" for i, c in enumerate(parabola)])
plt.plot(x, y, label=label)
plt.legend()
# Animated line
ax.set_xlim((TRACK_START_X, TRACK_END_X))
ax.set_ylim((-0.2, 1))
line, = ax.plot([], [], linestyle='None', marker="o", label="Frame: 0")
ax.legend()
# Animation settings
framerate = 16/1000
frame_count = 350
print(f"Total duration of animation is {framerate*frame_count}s")
def init():
line.set_data([], [])
return (line,)
x_pos = [0]
y_pos = [np.polyval(parabola, x_pos[0])]
vel = [0]
def animate(frame):
# Bruker matte4-funksjoner, heh
# part 1 - integrate acceleration to get velocity
# v'=sin(alpha)*9.81
# where functions ode_adaptive ask for x, think t for time instead.
current_x = x_pos[-1]
track_angle = np.arctan(-np.polyval(d_parabola, current_x))
def accell(t, v):
gravity = 9.81
braking_factor = 0.4
return (5/7)*gravity*np.sin(track_angle) - braking_factor*v
t_num, v_num = ode_adaptive(accell, x0=0, xend=framerate, y0=vel[-1], h0=framerate/2)
# Save velocity
vel.append(v_num[-1])
#part 2 - integrate velocity to get position
def velocity(t, x):
angle = np.arctan(-np.polyval(d_parabola, x))
return v_num[-1]*np.cos(angle)
t_num, x_num = ode_adaptive(velocity, x0=0, xend=framerate, y0=current_x, h0=framerate)
# Save the values
x_pos.append(x_num[-1])
y_pos.append(np.polyval(parabola, x_num[-1]))
# Update the animated point
line.set_data(x_pos[-1], y_pos[-1])
line.set_label(f"Frame {frame}, t={np.round(frame*framerate, 1)}. pos=({round(x_pos[-1],1)}, {round(y_pos[-1],1)}) ")
ax.legend()
return (line,) # Return all updated artists
anim = animation.FuncAnimation(fig, animate, init_func=init,
frames=frame_count, interval=framerate*1000,
blit=True)
rc('animation', html='jshtml')
return anim
# Running the animation function
def showAnimation():
parabola = [3, -11.3, 14.08, -6.508, 1.0768]
anim = obstacleCurveAnimation(parabola)
save_vid = False
if save_vid:
# Set up formatting for the movie files
Writer = animation.writers['ffmpeg']
writer = Writer(fps=1/framerate, metadata=dict(artist='Kristian'), bitrate=1800)
anim.save('out/1-simple-curve-ball.mp4', writer=writer)
print("Saved!")
return anim
print("Wait a few seconds for the animation to show")
anim = showAnimation()
if run_slow:
anim
Vi trenger et uttrykk som kan gi polynomer ut i fra høyden på midten.
def makePolynom(height):
points = [(0, 1), (0.5, 0), (1, height), (1.5, 0), (2, 1)]
x = [p[0] for p in points]
y = [p[1] for p in points]
return np.polyfit(x, y, len(points)-1)
def plotPolynomer():
steps = 100
x = np.linspace(TRACK_START_X, TRACK_END_X, steps)
heights = [-0.3, 0, 0.1, 0.3, 0.5]
polynoms = [makePolynom(h) for h in heights]
plt.xlim((0, 2))
for i, p in enumerate(polynoms):
y = np.polyval(p, x)
plt.plot(x, y, label=f"Height {heights[i]}")
plt.legend()
plt.show()
plotPolynomer()
print("Wait a few seconds for the animation to show")
if run_slow:
obstacleCurveAnimation(makePolynom(0.2))
a passing of the obstacle could be detected by looking at $x(t_1), x(t_2), v(t_1)$ and verifying that $x$ moved past the point $x=x_{obstacle}$ (test with $ x_{obstacle} \in \left(x(t_1), x(t_2)\right) $) while $v$ was positive for $x(t_1) < x_{obstacle}$ and negative for $x(t_1) > x_{obstacle}$.
# Loop over and analyse x and v
def makePasses():
parabola = makePolynom(0.2)
d_parabola = np.polyder(parabola)
steps = 100
framerate = 20/1000
frame_count = 300
t = [0]
x_pos = [0]
vel = [0]
for i in range(frame_count-1):
time = i*framerate
t.append(time)
current_x = x_pos[-1]
def accell(t, v):
gravity = 9.81
braking_factor = 0.4
track_angle = np.arctan(-np.polyval(d_parabola, current_x))
return (5/7)*gravity*np.sin(track_angle) - braking_factor*v
t_num, v_num = ode_adaptive(accell, x0=0, xend=framerate, y0=vel[-1], h0=0.2)
vel.append(v_num[-1])
#part 2
def velocity(t, x):
angle = np.arctan(-np.polyval(d_parabola, x))
return v_num[-1]*np.cos(angle)
t_num, x_num = ode_adaptive(velocity, x0=0, xend=framerate, y0=current_x, h0=0.2)
x_pos.append(x_num[-1])
# END matte 4
x = np.linspace(0, 1.5, steps)
y = np.polyval(parabola, x)
time_axis = np.linspace(0, frame_count*framerate, frame_count)
# Plot
fig, (x_ax, x_ax2, t_ax) = plt.subplots(3, figsize=[12, 8])
x_ax.plot(x, y, label=f"{parabola[0]}x^2 + {parabola[1]}x + {parabola[2]}")
x_ax.plot(x, np.arctan(-np.polyval(d_parabola, x)), label="Track Angle (rad)")
x_ax.plot(x, np.sin(np.arctan(-np.polyval(d_parabola, x))), label="Track Angle cosine")
x_ax.legend()
x_ax2.plot(x, -np.polyval(d_parabola, x), label="Track derivative (negated)")
x_ax2.legend()
t_ax.plot(time_axis, vel, label="Ball velocity")
t_ax.plot(time_axis, x_pos, label="Ball x")
t_ax.axhline(1, label="x_obstacle", color="orange")
t_ax.legend()
return (t, x_pos, vel)
def contains(a, b, obstacle):
return (a <= obstacle <= b) or (b <= obstacle <= a)
def detectPasses(times, x, velocities, obstacle_x):
to_chunk = deque(enumerate(velocities)) # [(index, velocity)]
chunks = [[]]
current_sign = np.sign([vel[1] for vel in to_chunk if vel[1] != 0][0])
while len(to_chunk) > 0:
i, v = to_chunk.popleft()
sign = np.sign(v)
if sign != current_sign and sign != 0:
current_sign = sign
chunks.append([])
chunks[-1].append((i, v, times[i]))
#print(f"Got {len(chunks)} chunks where speed is the same direction")
obstacle_passes = [chunk for chunk in chunks if len(chunk) > 2 and contains(x[chunk[0][0]], x[chunk[-1][0]], obstacle_x)]
return obstacle_passes
(t, x, v) = makePasses()
obstacle_x = 1
passes = detectPasses(t, x, v, obstacle_x)
for i, chunk in enumerate(passes):
print(f"Pass {i}:")
for j, vel, time in chunk:
print(f"{j}\t{time}\t{vel}")
def useClass():
ball = num_integ.Ball(mass=0.01, radius=0.01, start_x=0, start_y=0.7)
polynom = makePolynom(0.25)
track = num_integ.Track(polynom, start_height=1, obstacle_height=0.25, end_height=1)
integrator = num_integ.BallIntegrator(ball, track)
integrator.timestep = 0.016
integrator.braking_factor=0.8
steps = 600
print(f"Calculating to time {steps*integrator.timestep}s")
for step in range(steps):
integrator.step()
forceCalculator = num_integ.ForceCalculator(track, ball, integrator.time, integrator.position, integrator.velocity, integrator.acceleration)
passChecker = num_integ.ObstaclePassChecker(obstacle_x=1) # x defined in makePolynom
frictions, normalForces = forceCalculator.calculate()
flight = None
for i, n in enumerate(normalForces):
if n < 0:
flight = integrator.position[i]
break
# Plot stuff
fig, (ax0, ax1, ax2) = plt.subplots(3, figsize=(17, 9))
x = np.linspace(0, 2, 100)
ax0.plot(x, np.polyval(polynom, x))
if flight:
ax0.axvline(x=flight)
ax1.plot(integrator.time, integrator.position, label="Position", color="red")
ax1.plot(integrator.time, integrator.velocity, label="Velocity")
ax1.plot(integrator.time, integrator.acceleration, label="Acceleration")
ax1.axhline(1, label="x_obstacle", color="red")
ax1.legend()
ax2.plot(integrator.time, frictions, label="Friction")
ax2.plot(integrator.time, normalForces, label="Normal force")
ax2.legend()
useClass()
# TODO
(t, x, v) = makePasses()
obstacle_x = 1
passes = detectPasses(t, x, v, obstacle_x)
for i, chunk in enumerate(passes):
print(f"Pass {i}:")
for j, vel, time in chunk:
print(f"{j}\t{time}\t{vel}")
def count_passes_track1():
data = load_p1100902()
data = filter_values.calculate_track_values(data)
t = data[:,0]
x = data[:,1]
velocities = data[:,3]
accelerations = data[:,4]
fig, (plt1, plt2, plt3) = plt.subplots(3, 1, figsize=(12,9))
#plt1.set_xlim(0, 2.7)
#plt2.set_xlim(0, 2.7)
#plt3.set_xlim(0, 2.7)
plt1.plot(t, x, label="Measured position")
plt2.plot(t, velocities, label="Measured velocity")
plt3.plot(t, accelerations, label="Measured acceleration")
plt1.legend()
plt2.legend()
plt3.legend()
plt.show()
obstacle_x = 0.59890243
passes = detectPasses(t, x, velocities, obstacle_x)
print(f"Found {len(passes)} passes")
#for i, chunk in enumerate(passes):
# print(f"Pass {i}:")
# for j, vel, time in chunk:
# print(f"{j}\t{time}\t{vel}")
count_passes_track1()
def useReal():
measured_data = load_p1100902() # For comparison
(ball_x, ball_y), polynom, track_end_x = tracker_1()
obstacle_x = 0.59890243
#print(f"Roots at {np.roots(np.polyder(polynom))}")
print(f"Obstacle at {obstacle_x}")
ball = num_integ.Ball(mass=0.05, radius=0.0189/2, start_x=ball_x, start_y=ball_y)
track = num_integ.Track(polynom, start_height=0.3, obstacle_height=0.238, end_height=0.3)
integrator = num_integ.BallIntegrator(ball, track, braking_factor=0.11097230149082478) # Found later
integrator.timestep = 0.01
steps = 2000
print(f"Calculating to time {steps*integrator.timestep}s")
for step in range(steps):
integrator.step()
forceCalculator = num_integ.ForceCalculator(track, ball, integrator.time, integrator.position, integrator.velocity, integrator.acceleration)
passChecker = num_integ.ObstaclePassChecker(obstacle_x=1) # x defined in makePolynom
frictions, normalForces = forceCalculator.calculate()
flight = None
for i, n in enumerate(normalForces):
if n < 0:
flight = integrator.position[i]
break
passes = detectPasses(integrator.time, integrator.position, integrator.velocity, obstacle_x)
print(f"Found {len(passes)} passes")
# Plot stuff
fig, (ax0, ax1, ax2, ax3) = plt.subplots(4, figsize=(17, 12))
x = np.linspace(0, track_end_x, 100)
ax0.plot(x, np.polyval(polynom, x), label="Track")
ax0.axvline(obstacle_x, label="Obstacle", color="red")
ax0.legend()
if flight:
ax0.axvline(x=flight, label="Error! Liftoff/flight here")
ax1.plot(integrator.time, integrator.position, label="Position", color="red")
ax1.plot(integrator.time, integrator.velocity, label="Velocity")
ax1.plot(integrator.time, integrator.acceleration, label="Acceleration")
ax1.axhline(obstacle_x, label="x_obstacle", color="red")
ax1.legend()
ax2.plot(integrator.time, frictions, label="Friction")
ax2.plot(integrator.time, normalForces, label="Normal force")
ax2.legend()
ax3.plot(measured_data[:,0], measured_data[:,1], label="Measured position", color="black")
ax3.plot(integrator.time, integrator.position, label="Position", color="red")
ax3.legend()
useReal()
def calculate_measured_values(obstacle_x = 0.59890243):
data = load_p1100902()
velocities = [0]
for i, (t, x, y) in enumerate(data[1:]):
(prev_x, prev_y) = data[i-1][1:]
distance = (x-prev_x)
delta_time = t - data[i-1][0]
velocities.append(distance/delta_time)
t = data[:,0]
x = data[:,1]
passes = detectPasses(t, x, velocities, obstacle_x)
return t, x, velocities
def simulate_with_braking(braking_factor=0.1119893, obstacle_x = 0.59890243, steps = 600, timestep=0.016):
"""
:return: False|int|tuple, int if passes are invalid
"""
(ball_x, ball_y), polynom, track_end_x = tracker_1()
ball = num_integ.Ball(mass=0.05, radius=0.0189/2, start_x=ball_x, start_y=ball_y)
track = num_integ.Track(polynom, start_height=0.3, obstacle_height=0.238, end_height=0.3)
integrator = num_integ.BallIntegrator(ball, track, braking_factor=braking_factor)
integrator.timestep = timestep
for step in range(steps):
integrator.step()
forceCalculator = num_integ.ForceCalculator(track, ball, integrator.time, integrator.position, integrator.velocity, integrator.acceleration)
passChecker = num_integ.ObstaclePassChecker(obstacle_x)
passes = detectPasses(integrator.time, integrator.position, integrator.velocity, obstacle_x)
if len(passes) != 2:
print(f"Invalid braking factor {braking_factor}, {len(passes)} passes")
return len(passes)
frictions, normalForces = forceCalculator.calculate()
for i, n in enumerate(normalForces):
if n < 0:
print(f"Invalid setup. Ball flies at force index {i}")
return False
return (integrator.time, integrator.position, integrator.velocity)
def find_error(measured_t, measured_x, calc_t, calc_x):
"""
:return: float, the error
"""
# 1: interpolate to match t values
# 2: diff x values at interpolated values
if len(measured_x) != len(calc_x):
raise Exception(f"Must be same size! {len(measured_x)} vs {len(calc_x)}")
return np.square(np.sum(np.square(calc_x - measured_x))/len(measured_x))
def find_close_match():
(meas_t, meas_x, meas_v) = calculate_measured_values()
braking_factor = 0.13 #0.111
low_factor = 0.095
high_factor = 0.4
last_error = np.inf
smallest_error = np.inf
best_factor = braking_factor
compare_size = 600
frame_rate_seconds=0.01
errors = []
for run_number in range(30):
print(f"Run {run_number}. Factor {braking_factor}" + "-"*20)
simulated = simulate_with_braking(braking_factor=braking_factor, steps=compare_size-1, timestep=frame_rate_seconds)
if isinstance(simulated, bool) and simulated == False:
# Flight
low_factor = braking_factor
braking_factor = (high_factor + low_factor) / 2
elif isinstance(simulated, int):
# Invalid pass number
passes = simulated
print(f"Found {passes} passes with factor {braking_factor}. Range ({low_factor}, {high_factor})")
if passes < 2:
high_factor = braking_factor
elif passes > 2:
low_factor = braking_factor
braking_factor = (high_factor + low_factor)/2
print(f"Adjusted factors: {braking_factor} in range ({low_factor}, {high_factor})")
else:
# Normal case
(calc_t, calc_x, calc_v) = simulated
error = find_error(meas_t[:compare_size], meas_x[:compare_size], calc_t, calc_x)
errors.append((braking_factor, error))
if error < smallest_error:
# New best case
smallest_error = error
best_factor = braking_factor
print(f"Found better factor: {braking_factor} with error {error}")
# Assuming error is in a valley, from previous graphed runs
if error > last_error:
# Error too far from best case, go back towards best
braking_factor = (best_factor + braking_factor) / 2
else:
# Look around best case
braking_factor = random.uniform(0.9, 1.1)*best_factor
last_error = error
errors = np.array(sorted(errors, key=lambda err: err[0]))
plt.plot(errors[:,0], errors[:,1], label="Error")
plt.legend()
plt.show()
print("-"*100)
print(f"Best value found: {best_factor} (error={smallest_error})")
# 0.11097230149082478 (error=4.3536731384222186e-07)
# 0.11098230149082479 (error=3.340337453326927e-06) with 800 compare_size
find_close_match()
# Curve with obstacle animation
def realObstacleCurveAnimation():
fig, ax = plt.subplots()
# Background path using the curve
(ball_x, ball_y), parabola, track_end_x = tracker_1()
plt.xlim((0, track_end_x))
steps = 100
x = np.linspace(0, track_end_x, steps)
y = np.polyval(parabola, x)
plt.plot(x, y, label="Track")
plt.legend()
# Animated line
ax.set_xlim((0, track_end_x))
ax.set_ylim((0, 0.33))
line, = ax.plot([], [], linestyle='None', marker="o", label="Frame: 0")
ax.legend()
# Animation settings
framerate = 1/25
frame_count = 150
print(f"Total duration of animation is {framerate*frame_count}s")
obstacle_x = 0.59890243
#print(f"Roots at {np.roots(np.polyder(polynom))}")
print(f"Obstacle at {obstacle_x}")
ball = num_integ.Ball(mass=0.05, radius=0.0189/2, start_x=ball_x, start_y=ball_y)
track = num_integ.Track(parabola, start_height=0.3, obstacle_height=0.238, end_height=0.3)
integrator = num_integ.BallIntegrator(ball, track, braking_factor=0.11097230149082478) # Found later
integrator.timestep = framerate
def init():
line.set_data([], [])
return (line,)
x_pos = [0]
y_pos = [np.polyval(parabola, x_pos[0])]
def animate(frame):
integrator.step()
x = integrator.position[-1]
y = np.polyval(parabola, x)
line.set_data(x, y)
line.set_label(f"Frame {frame}, t={np.round(frame*framerate, 1)}. pos=({round(x,1)}, {round(y,1)}) ")
ax.legend()
return (line,) # Return all updated artists
anim = animation.FuncAnimation(fig, animate, init_func=init,
frames=frame_count, interval=framerate*1000,
blit=True)
rc('animation', html='jshtml')
return anim
def showAnimation():
anim = realObstacleCurveAnimation()
save_vid = False
if save_vid:
# Set up formatting for the movie files
Writer = animation.writers['ffmpeg']
writer = Writer(fps=25, metadata=dict(artist='Kristian'), bitrate=1800)
anim.save('out/2-real-track-data.mp4', writer=writer)
print("Saved!")
return anim
if run_slow:
showAnimation()